# -*- encoding: utf-8 -*-
"""
@File    :   obstacle.py

@Contact :   2055466817@qq.com

@Modify Time :   2020/12/2 上午7:24

@Author :   赵方国
"""
from ImagProcess import getImag
from naoqi import ALProxy
import cv2 as cv
import random
import almath
from cmath import pi
import time


def obstacle(robotIP, PORT, angleINIT):
    """
    简单的执行从右边绕过障碍物的动作，走的是方波形式,未使用

    @param robotIP:
    @param PORT:
    @param angleINIT:传入初始角度用于校准
    @return:
    """
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    tts.say("Now is the left obstacle")

    # 进行角度的校准
    # angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    correctAngle(robotIP, PORT, angleINIT)

    # 绕开障碍物
    # motionProxy.moveTo(0.2, -0.2, -pi / 2, smallTurnStep)  # TODO 此处的值需要结合阈值进行调整

    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.3, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    correctAngle(robotIP, PORT, angleINIT)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.55, 0, 0, smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    # 绕开障碍物
    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.3, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)

    # 进行角度的校准
    correctAngle(robotIP, PORT, angleINIT)


def obstacle2(robotIP, PORT, angleINIT):
    """
    简单的执行从左边绕过障碍物的动作，走的是方波形式，未使用

    @param robotIP:
    @param PORT:
    @param angleINIT:传入初始角度用于校准
    @return:
    """
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    tts.say("Now is the RIGHT obstacle")

    # 进行角度的校准
    correctAngle(robotIP, PORT, angleINIT)
    # angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    # print 'angle: ' + str(angle), 'trurned: ' + str(-(angle - angleINIT))
    # if abs(angle - angleINIT) > 3 * pi / 180:
    #     motionProxy.moveTo(0, 0, -(angle - angleINIT))
    #     print 'turn'

    # 绕开障碍物
    # motionProxy.moveTo(0.2, -0.2, -pi / 2, smallTurnStep)  # TODO 此处的值需要结合阈值进行调整

    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.3, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    # angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    # print 'angle: ' + str(angle), 'trurned: ' + str(-(angle - angleINIT))
    # if abs(angle - angleINIT) > 3 * pi / 180:
    #     motionProxy.moveTo(0, 0, -(angle - angleINIT))
    #     print 'turn'
    correctAngle(robotIP, PORT, angleINIT)
    correctAngle(robotIP, PORT, angleINIT)
    motionProxy.moveTo(0.55, 0, 0, smallTurnStep)

    # 进行角度的校准

    correctAngle(robotIP, PORT, angleINIT)
    # angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    # print 'angle: ' + str(angle), 'trurned: ' + str(-(angle - angleINIT))
    # if abs(angle - angleINIT) > 5 * pi / 180:
    #     motionProxy.moveTo(0, 0, -(angle - angleINIT))
    #     print 'turn'

    # 绕开障碍物
    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    # angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    # print 'angle: ' + str(angle), 'trurned: ' + str(-(angle - angleINIT))
    # if abs(angle - angleINIT) > 93 * pi / 180 or abs(angle - angleINIT) < 87 * pi / 180:
    #     motionProxy.moveTo(0, 0, -(angle - angleINIT + 90 * pi / 180))
    #     print 'turn'

    motionProxy.moveTo(0.3, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)

    # 进行角度的校准
    correctAngle(robotIP, PORT, angleINIT)
    # angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    # print 'angle: ' + str(angle), 'trurned: ' + str(-(angle - angleINIT))
    # if abs(angle - angleINIT) > 5 * pi / 180:
    #     motionProxy.moveTo(0, 0, -(angle - angleINIT))
    #     print 'turn'


def obstacleHENGZHEZOU(robotIP, PORT, angleINIT, flag):
    """
    进行从右边绕过障碍物的动作，采用横着走的方式,使用moveToward函数和距离判断，moveToward走的稳一些
    @param robotIP: 机器人IP
    @param PORT: 9559
    @param angleINIT:初始角度值用于校准
    @return:
    """
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    tts.say("Now is the left obstacle")

    # 进行角度的校准
    # angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    correctAngle(robotIP, PORT, angleINIT)

    # 绕开障碍物
    # motionProxy.moveTo(0.2, -0.2, -pi / 2, smallTurnStep)  # TODO 此处的值需要结合阈值进行调整

    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
    INITROBO = initRobotPosition
    while True:
        motionProxy.moveToward(0, 0.4, 0, lower_steps)
        endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
        robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
        robotMove1 = almath.pose2DInverse(INITROBO) * endRobotPosition

        robotMove = almath.Pose2D.toVector(robotMove)
        robotMove1 = almath.Pose2D.toVector(robotMove1)

        robotMove = robotMove[1]
        robotMove1 = robotMove1[1]
        print "robotMove1:"
        print robotMove1
        print "robotMove" + str(robotMove)
        if abs(robotMove) > 0.1:
            correctAngle(robotIP, PORT, angleINIT)
            initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
        if abs(robotMove1) > 0.55:
            break
    correctAngle(robotIP, PORT, angleINIT)
    # correctAngle(robotIP, PORT, angleINIT)
    # correctAngle(robotIP, PORT, angleINIT)

    # motionProxy.moveTo(0.55, 0, 0, smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    # 绕开障碍物
    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)

    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))

    while True:
        name = str(random.randint(0, 1000))
        data = getImag(robotIP, PORT, 1, flag, name)
        motionProxy.moveToward(0, -0.4, 0, lower_steps)
        # correctAngle(robotIP, PORT, angleINIT)
        if data[0] + data[1] + data[2] + data[3] == 0:
            break
        endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
        robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
        robotMove = almath.Pose2D.toVector(robotMove)
        robotMove = robotMove[1]
        print robotMove
        if robotMove > 0.2:
            correctAngle(robotIP, PORT, angleINIT)
            initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))

    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)

    # 进行角度的校准
    correctAngle(robotIP, PORT, angleINIT)


def obstacleHENGZHEZOU2(robotIP, PORT, angleINIT, flag):
    """
    进行从左边绕过障碍物的动作，采用横着走的方式,使用moveToward函数和距离判断，moveToward走的稳一些
    @param flag: 绕柱子的颜色
    @param robotIP: 机器人IP
    @param PORT: 9559
    @param angleINIT:初始角度值用于校准
    @return:
    """
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    tts.say("Now is the left obstacle")

    # 进行角度的校准
    # angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    correctAngle(robotIP, PORT, angleINIT)

    # 绕开障碍物
    # motionProxy.moveTo(0.2, -0.2, -pi / 2, smallTurnStep)  # TODO 此处的值需要结合阈值进行调整

    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
    INITROBO = initRobotPosition
    while True:
        motionProxy.moveToward(0, -0.4, 0, lower_steps)
        endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
        robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
        robotMove1 = almath.pose2DInverse(INITROBO) * endRobotPosition

        robotMove = almath.Pose2D.toVector(robotMove)
        robotMove1 = almath.Pose2D.toVector(robotMove1)

        robotMove = robotMove[1]
        robotMove1 = robotMove1[1]
        print robotMove1
        if abs(robotMove) > 0.1:
            correctAngle(robotIP, PORT, angleINIT)
            initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
        if abs(robotMove1) > 0.55:
            break
    correctAngle(robotIP, PORT, angleINIT)
    # correctAngle(robotIP, PORT, angleINIT)
    # correctAngle(robotIP, PORT, angleINIT)

    # motionProxy.moveTo(0.55, 0, 0, smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    # 绕开障碍物
    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)

    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))

    while True:
        name = str(random.randint(0, 1000))
        data = getImag(robotIP, PORT, 1, flag, name)
        motionProxy.moveToward(0, 0.4, 0, lower_steps)
        # correctAngle(robotIP, PORT, angleINIT)
        if data[0] + data[1] + data[2] + data[3] == 0:
            break
        endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
        robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
        robotMove = almath.Pose2D.toVector(robotMove)
        robotMove = robotMove[1]
        print robotMove
        if robotMove > 0.2:
            correctAngle(robotIP, PORT, angleINIT)
            initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)

    # 进行角度的校准
    correctAngle(robotIP, PORT, angleINIT)


def correctAngle(robotIP, PORT, angleINIT):
    """
    角度校准函数！进行直线和90度旋转的校准工作，可以说是整个障碍跑成功的关键所在！！！
    @param robotIP: 机器人IP
    @param PORT: 9559
    @param angleINIT:初始角度，应该传入一个定值
    @return:
    """
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    # print 'angle: ' + str(angle), 'trurned: ' + str(-(angle - angleINIT))
    print "当前角度:" + str(angle)
    print "初始角度：" + str(angleINIT)
    angleREcorrect = (angle - angleINIT)
    print "待校准角度:" + str(angleREcorrect)
    if abs(angleREcorrect) < 1 * pi / 180:
        print "do not need to correct"
        return True
    if abs(angleREcorrect) > pi:
        if angleREcorrect > 0:
            angleCorrect = abs(angle - angleINIT) - pi - pi / 2
        else:
            angleCorrect = -(abs(angle - angleINIT) - pi - pi / 2)
        motionProxy.moveTo(0, 0, -angleCorrect)
        print "大于 pi"
        print "turn to " + str(- angleCorrect)
        return True

    if 1 * pi / 180 < abs(angleREcorrect) < 49 * pi / 180:
        motionProxy.moveTo(0, 0, -(angle - angleINIT))
        print 'turn'
        print str(-(angle - angleINIT))
        return True
    if abs(angleREcorrect) > 93 * pi / 180 or ((87 * pi / 180) > abs(angleREcorrect) > (50 * pi / 180)):
        if angleREcorrect < 0:
            motionProxy.moveTo(0, 0, (abs(angleREcorrect) - 90 * pi / 180))
        else:
            motionProxy.moveTo(0, 0, -(abs(angleREcorrect) - 90 * pi / 180))
        print "大于90度，校准"
        print 'turn to ' + str((abs(angleREcorrect) - 90 * pi / 180))
        return True


if __name__ == '__main__':
    robotIP = "169.254.15.227"
    PORT = 9559
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    while True:
        # angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value")
        # angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/GyroscopeX/Sensor/Value")
        angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AccelerometerX/Sensor/Value")
        print angle
        # time.sleep(2)

    # obstacle(robotIP, 9559, angle)
    # obstacle2(robotIP, PORT, angle)
    # obstacle(robotIP, PORT, angle)
    # motionProxy.rest()
